#pragma once
// ============================================================================
// 工程：SLAM（纯反光柱识别 + 定位）
// 文件：include/SLAM/common.hpp
// 说明：通用类型/结构体/头文件统一引入。
// 本工程仅进行反光柱识别与定位，不包含任何栅格建图功能。
// ============================================================================
#include <memory>
#include <vector>
#include <string>
#include <unordered_map>
#include <mutex>
#include <optional>
#include <tuple>
#include <cmath>
#include <limits>
#include <algorithm>
#include <chrono>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <deque>
#include <numeric>
#include <map>



#include <rmw/qos_profiles.h>  //v1.1
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <cartographer_ros_msgs/msg/landmark_list.hpp>
#include <message_filters/subscriber.h>
#include <ceres/ceres.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <std_msgs/msg/header.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/create_timer_ros.h> 
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/utils.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>


namespace SLAM { // 命名空间改为 SLAM

// 点云类型定义（XYZI）
using PointT = pcl::PointXYZI;
using CloudT = pcl::PointCloud<PointT>;

// 当前帧的反光柱观测（基座系：base_link）
struct Reflector {
  double x{0}, y{0};      // 圆心（单位：米）
  double radius{0};       // 圆半径（米，如无法估计可为0）
  double strength{0};     // 强度（由聚类/圆拟合阶段统计得到的代表值）
};

// 地图系（map）下维护的全局地标
struct Landmark {
  int id{-1};
  double x{0}, y{0};
  Cov2 P{0.2, 0.0, 0.2};          // 初始协方差
  double radius{0.08};
  double strength{0.0};
  int hits{0};
  double last_seen_sec{0.0};
  bool confirmed{false};
};

// 观测与地标的匹配对
struct MatchPair { 
  int obs_index{-1}; 
  int lm_index{-1}; 
  double d2{1e9}; 
};

// 简单 2x2 协方差工具
struct Cov2 {
  double xx{0.05}, xy{0.0}, yy{0.05}; // 初值可通过参数修改
};

struct GatedCost {
  std::vector<std::vector<double>> cost; // cost[i][j] = d2 or inf
  std::vector<std::vector<bool>>   ok;   // ok[i][j] = 是否通过门限
};


inline double angNorm(double a) {
  while (a > M_PI) a -= 2*M_PI;
  while (a < -M_PI) a += 2*M_PI;
  return a;
}


inline double mahalanobis2(double dx, double dy, const Cov2& C) {
  const double det = C.xx*C.yy - C.xy*C.xy;
  if (det <= 1e-12) return std::numeric_limits<double>::infinity();
  const double inv_xx =  C.yy / det;
  const double inv_xy = -C.xy / det;
  const double inv_yy =  C.xx / det;
  return dx*(inv_xx*dx + inv_xy*dy) + dy*(inv_xy*dx + inv_yy*dy);
}

double mahalanobis_d2(const Eigen::Vector2d& z,
                      const Eigen::Vector2d& mu,
                      const Eigen::Matrix2d& Pm,
                      const Eigen::Matrix2d& R) const {
  const Eigen::Vector2d v = z - mu;
  const Eigen::Matrix2d S = Pm + R;
  return v.transpose() * S.inverse() * v;
}

// 小工具：根据 r/I 给一个 2x2 的测量噪声
Eigen::Matrix2d R_from_range_intensity(const Reflector& o) const {
  const double r = std::hypot(o.x, o.y);
  const double I = std::max(1.0, o.strength);
  // 经验模型：近距离/高强度噪声更小；远距离/弱强度噪声更大
  double sigma = std::max(0.005, matcher_params_.obs_sigma_xy * (1.0 + 0.25*r + 30.0/I));
  Eigen::Matrix2d R = Eigen::Matrix2d::Identity() * (sigma*sigma);
  return R;
}

// ===== 预门：构造马氏门限内的候选与 d^2 代价矩阵（inf=不可选）=====

GatedCost build_gated_cost(const std::vector<Reflector>& obs_map,
                           const std::vector<Landmark>& lms_confirm,
                           const std::vector<Cov2>& covs) const {
  const double gate2 = matcher_params_.mahalanobis_gate2; // 例如 5.99(95%), 9.21(99%)
  GatedCost M;
  M.cost.assign(obs_map.size(), std::vector<double>(lms_confirm.size(),
                    std::numeric_limits<double>::infinity()));
  M.ok.assign(obs_map.size(), std::vector<bool>(lms_confirm.size(), false));

  for (size_t i=0;i<obs_map.size();++i) {
    Eigen::Vector2d z(obs_map[i].x, obs_map[i].y);
    Eigen::Matrix2d R = R_from_range_intensity(obs_map[i]);
    for (size_t j=0;j<lms_confirm.size();++j) {
      Eigen::Vector2d mu(lms_confirm[j].x, lms_confirm[j].y);
      Eigen::Matrix2d Pm; Pm << covs[j].xx, covs[j].xy, covs[j].yx, covs[j].yy;
      double d2 = mahalanobis_d2(z, mu, Pm, R);
      if (d2 <= gate2) { M.cost[i][j] = d2; M.ok[i][j] = true; }
    }
  }
  return M;
}

// ===== 最近邻粗配（在“已经通过马氏预门”的候选里做互为最近邻）=====

std::vector<MatchPair> mutual_nearest_pairs(const GatedCost& M) const {
  const size_t O = M.cost.size();
  if (O==0) return {};
  const size_t L = M.cost[0].size();
  std::vector<int> nn_obs_to_lm(O, -1);
  std::vector<int> nn_lm_to_obs(L, -1);

  // obs -> nearest lm (按 d^2)
  for (size_t i=0;i<O;++i) {
    double best = 1e18; int bestj = -1;
    for (size_t j=0;j<L;++j) if (M.ok[i][j] && M.cost[i][j] < best) {
      best = M.cost[i][j]; bestj = (int)j;
    }
    nn_obs_to_lm[i] = bestj;
  }
  // lm -> nearest obs
  for (size_t j=0;j<L;++j) {
    double best = 1e18; int besti = -1;
    for (size_t i=0;i<O;++i) if (M.ok[i][j] && M.cost[i][j] < best) {
      best = M.cost[i][j]; besti = (int)i;
    }
    nn_lm_to_obs[j] = besti;
  }

  // 互为最近邻 → 粗配
  std::vector<MatchPair> pairs;
  for (size_t i=0;i<O;++i) {
    int j = nn_obs_to_lm[i];
    if (j >= 0 && j < (int)L && nn_lm_to_obs[j] == (int)i) {
      pairs.push_back({(int)i, j, M.cost[i][j]});
    }
  }
  return pairs;
}

} // namespace SLAM
